#include "helper.h"

#ifdef FULLSTEP
#ifdef HALFDRIVE
const uint8_t steps[4] = {
						_BV(A3),
						_BV(A2),
						_BV(A4),
						_BV(A1)
					};
#else
const uint8_t steps[4] = {
						(_BV(A3) | _BV(A2)),
						(_BV(A2) | _BV(A4)),
						(_BV(A4) | _BV(A1)),
						(_BV(A1) | _BV(A3))
					};
#endif
#else
const uint8_t steps[8] = {
						_BV(A3),
						(_BV(A3) | _BV(A2)),
						_BV(A2),
						(_BV(A2) | _BV(A4)),
						_BV(A4),
						(_BV(A4) | _BV(A1)),
						_BV(A1),
						(_BV(A1) | _BV(A3))
					};
#endif

/*******************************************************************/

/*
 * atol()
 * Small replacement of atol in StdLib. Used to convert numbers in
 * strings to numbers
 *
 */

int16_t atolong(unsigned char* in,uint8_t *offset, uint8_t len) {

	uint8_t i=0;
	int16_t atol=0;

	for (;i<len;i++) {
		if (in[i+(*offset)] < 45) break;
		atol *= 10;
		atol += (in[i+(*offset)] > 47 ? in[i+(*offset)] - '0' : 0);
	}

	if (in[*offset] == '-') atol *= -1;
	*offset += i;

	return atol;
}

/*
 Step()

 Outputs the correct step to the L293D selected
 by the enable pin
*/
inline void SetStep(uint8_t Step)
{
	PORTC = steps[Step];
}
/*******************************************************************/
/*
 _dummyMove()

 Move each axis 25 steps to syncronise hardware
 with step inside axis structure
*/
void _dummyMove(uint8_t CurAxis)
{
  // Alle Motoren einmal anfahren um cur_step mit Motor zu synronisieren
 uint8_t tmp;
 for (tmp=15; tmp>0; tmp--)
 {
 	Mot_plus(CurAxis);
	mindelay += 10;
 }
 for (tmp=15; tmp>0; tmp--)
 {
    Mot_minus(CurAxis);
	mindelay += 10;
 }
}

void initializeAxis(uint8_t CurAxis) {

	int i, count = 0;

	disableLimits();

	while ((PIND & _BV(Axis[CurAxis].iSwitch)) != 0) {
		Mot_minus(CurAxis);
	}

	_delay_ms(100);

	for (i=0; i<MARGIN; i++) {
		Mot_plus(CurAxis);
	}

	while ((PIND & _BV(Axis[CurAxis].iSwitch)) != 0) {
		count ++;
		Mot_plus(CurAxis);
	}

	_delay_ms(100);

	Axis[CurAxis].maxSteps = count - 2*MARGIN;

	for (i=0; i<(count-MARGIN); i++) {
		Mot_minus(CurAxis);
	}


	Axis[CurAxis].Pos = 0;
	enableLimits();

}

void initialize() {

	initializeAxis(0);
	initializeAxis(1);

}

/*******************************************************************/
void SetMindelay(uint8_t newdelay)
{
 cli();
 mindelay = newdelay;
 sei();
}
/*******************************************************************/
/*
 Mot_plus()

 Moves motor one step forward
*/
void Mot_plus(uint8_t curAxis)
{
 while (mindelay > 0)	 	sleep_mode();

 // Turn off all Motors
 PORTB &= ~(_BV((X.iMotor+2)) | _BV((Y.iMotor+2)) | _BV((Z.iMotor+2)));

 // Set new step
 SetStep(++Axis[curAxis].iStep);
 
 // Enable Motor
 PORTB |= (1 << ((Axis[curAxis].iMotor)+2));
 
 // This delay must pass before another driver may be activated
 // 1 / (mindelay * F_CPU/TIMER_PRESCALE/(255-Preload)) = OnTime in seconds 
 SetMindelay( 3);
 
 Axis[curAxis].Pos++;
}

/*
 Mot_minus()

 Moves motor one step backwards
*/
void Mot_minus(uint8_t curAxis)
{
 while (mindelay > 0)	 	sleep_mode();

 // Turn off all Motors
 PORTB &= ~(_BV((X.iMotor+2)) | _BV((Y.iMotor+2)) | _BV((Z.iMotor+2)));

 // Set new step
 SetStep(--Axis[curAxis].iStep);
 
 // Enable Motor
 PORTB |= (1 << ((Axis[curAxis].iMotor)+2));
 
 // This delay must pass before another driver may be activated
 // 1 / (mindelay * F_CPU/TIMER_PRESCALE/(255-Preload)) = OnTime in seconds 
 SetMindelay( 3);
 
 Axis[curAxis].Pos--;
}
/*******************************************************************/

/*
 Move to X, Y, Z	NOT INTERPOLATED!
*/
inline void putpixel(int16_t newX, int16_t newY, int16_t newZ)
{
 while((newX != X.Pos) || (newY != Y.Pos) || (newZ != Z.Pos))
 {
  while (mindelay > 0)		sleep_mode();

  if(newX != X.Pos) 
  { 
	SetStep(X.iStep);

  	newX > X.Pos ? Mot_plus (0) : Mot_minus (0);
  } else SetMindelay ( 5);
 
  while (mindelay > 0)		sleep_mode();
  if(newY != Y.Pos) 
  { 
	SetStep(Y.iStep);

  	newY > Y.Pos ? Mot_plus (1) : Mot_minus (1);
  } else SetMindelay ( 5);
  
  while (mindelay > 0)		sleep_mode();
  if(newZ != Z.Pos)
  { 
	SetStep(Z.iStep);

  	newZ > Z.Pos ? Mot_plus (2) : Mot_minus (2);
  } else SetMindelay ( 5);
 }
}
/*******************************************************************/
